/** @file
  Rockchip eDP Driver.

  Copyright (c) 2022 Rockchip Electronics Co. Ltd.

  SPDX-License-Identifier: BSD-2-Clause-Patent

**/

#include <Library/DebugLib.h>
#include <Uefi/UefiBaseType.h>
#include <Library/IoLib.h>
#include <Library/TimerLib.h>
#include <Library/BaseLib.h>
#include <Library/MemoryAllocationLib.h>
#include <Library/AnalogixDpLib.h>

#define COMMON_INT_MASK_1  0
#define COMMON_INT_MASK_2  0
#define COMMON_INT_MASK_3  0
#define COMMON_INT_MASK_4  (HOTPLUG_CHG | HPD_LOST | PLUG)
#define INT_STA_MASK       INT_HPD

VOID
AnalogixDpRegWrite (
  OUT struct  AnalogixDpDevice  *Dp,
  IN  UINT32                    Offset,
  IN  UINT32                    Value
  )
{
  MmioWrite32 (Dp->Base + Offset, Value);
}

UINT32
AnalogixDpRegRead (
  OUT struct  AnalogixDpDevice  *Dp,
  IN  UINT32                    Offset
  )
{
  return MmioRead32 (Dp->Base + Offset);
}

VOID
AnalogixDpStopVideo (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_VIDEO_CTL_1);
  Reg &= ~VIDEO_EN;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_VIDEO_CTL_1, Reg);
}

VOID
AnalogixDpSetLaneMap (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  // Todo
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_LANE_MAP, 0xE4);
}

VOID
AnalogixDpEnableVideoMute (
  OUT struct AnalogixDpDevice  *Dp,
  BOOLEAN                      Enable
  )
{
  UINT32  Reg;

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_VIDEO_CTL_1);
  if (Enable) {
    Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_VIDEO_CTL_1);
    Reg |= HDCP_VIDEO_MUTE;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_VIDEO_CTL_1, Reg);
  } else {
    Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_VIDEO_CTL_1);
    Reg &= ~HDCP_VIDEO_MUTE;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_VIDEO_CTL_1, Reg);
  }
}

VOID
AnalogixDpLaneSwap (
  OUT struct AnalogixDpDevice  *Dp,
  BOOLEAN                      Enable
  )
{
  // Todo
  UINT32  Reg;

  if (Enable) {
    Reg = LANE3_MAP_LOGIC_LANE_0 | LANE2_MAP_LOGIC_LANE_1 |
          LANE1_MAP_LOGIC_LANE_2 | LANE0_MAP_LOGIC_LANE_3;
  } else {
    Reg = LANE3_MAP_LOGIC_LANE_3 | LANE2_MAP_LOGIC_LANE_2 |
          LANE1_MAP_LOGIC_LANE_1 | LANE0_MAP_LOGIC_LANE_0;
  }

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_LANE_MAP, Reg);
}

VOID
AnalogixDpReset (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  AnalogixDpStopVideo (Dp);
  AnalogixDpEnableVideoMute (Dp, 0);

  Reg = MASTER_VID_FUNC_EN_N | SLAVE_VID_FUNC_EN_N |
        AUD_FIFO_FUNC_EN_N | AUD_FUNC_EN_N |
        HDCP_FUNC_EN_N | SW_FUNC_EN_N;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_FUNC_EN_1, Reg);

  Reg = SSC_FUNC_EN_N | AUX_FUNC_EN_N |
        SERDES_FIFO_FUNC_EN_N |
        LS_CLK_DOMAIN_FUNC_EN_N;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_FUNC_EN_2, Reg);

  NanoSecondDelay (30000);

  AnalogixDpSetLaneMap (Dp);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_1, 0x0);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_2, 0x40);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_3, 0x0);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_4, 0x0);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PKT_SEND_CTL, 0x0);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_HDCP_CTL, 0x0);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_HPD_DEGLITCH_L, 0x5e);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_HPD_DEGLITCH_H, 0x1a);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_LINK_DEBUG_CTL, 0x10);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PHY_TEST, 0x0);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_VIDEO_FIFO_THRD, 0x0);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUDIO_MARGIN, 0x20);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_M_VID_GEN_FILTER_TH, 0x4);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_M_AUD_GEN_FILTER_TH, 0x2);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SOC_GENERAL_CTL, 0x00000101);
}

VOID
AnalogixDpSwreset (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  /* SW_RESET */
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_TX_SW_RESET, RESET_DP_TX);
}

VOID
AnalogixDpInitAnalogParam (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg = TX_TERMINAL_CTRL_50_OHM;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_ANALOG_CTL_1, Reg);

  Reg = SEL_24M | TX_DVDD_BIT_1_0625V;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_ANALOG_CTL_2, Reg);

  Reg = REF_CLK_24M;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_REG_1, Reg);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_REG_2, 0x99);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_REG_3, 0x40);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_REG_4, 0x58);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_REG_5, 0x22);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_BIAS, 0x44);

  Reg = DRIVE_DVDD_BIT_1_0625V | VCO_BIT_600_MICRO;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_ANALOG_CTL_3, Reg);

  Reg = PD_RING_OSC | AUX_TERMINAL_CTRL_50_OHM |
        TX_CUR1_2X | TX_CUR_16_MA;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_FILTER_CTL_1, Reg);

  Reg = CH3_AMP_400_MV | CH2_AMP_400_MV |
        CH1_AMP_400_MV | CH0_AMP_400_MV;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_TX_AMP_TUNING_CTL, Reg);

  MicroSecondDelay (10);
}

VOID
AnalogixDpInitInterrupt (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  /* Set interrupt pin assertion polarity as high */
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_CTL, INT_POL1 | INT_POL0);

  /* Clear pending Regisers */
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_STA_1, 0xff);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_STA_2, 0x4f);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_STA_3, 0xe0);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_STA_4, 0xe7);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA_MASK, 0x63);

  /* 0:mask,1: unmask */
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_1, 0x00);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_2, 0x00);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_3, 0x00);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_4, 0x00);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA_MASK, 0x00);
}

VOID
AnalogixDpEnableSwFunction (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_FUNC_EN_1);
  Reg &= ~SW_FUNC_EN_N;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_FUNC_EN_1, Reg);
}

VOID
AnalogixDpConfigInterrupt (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  /* 0: mask, 1: unmask */
  Reg = COMMON_INT_MASK_1;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_1, Reg);
  Reg = COMMON_INT_MASK_2;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_2, Reg);

  Reg = COMMON_INT_MASK_3;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_3, Reg);

  Reg = COMMON_INT_MASK_4;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_MASK_4, Reg);
  Reg = INT_STA_MASK;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA_MASK, Reg);
}

VOID
AnalogixDpSetAnalogPowerDown (
  OUT struct AnalogixDpDevice  *Dp,
  enum AnalogPowerBlock        Block,
  BOOLEAN                      Enable
  )
{
  UINT32  Reg;
  UINT32  PhyPdAddr = ANALOGIX_DP_PD;

  // Todo
  switch (Block) {
    case AUX_BLOCK:
      if (Enable) {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg |= AUX_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg &= ~AUX_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      }

      break;
    case CH0_BLOCK:
      if (Enable) {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg |= CH0_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg &= ~CH0_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      }

      break;
    case CH1_BLOCK:
      if (Enable) {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg |= CH1_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg &= ~CH1_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      }

      break;
    case CH2_BLOCK:
      if (Enable) {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg |= CH2_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg &= ~CH2_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      }

      break;
    case CH3_BLOCK:
      if (Enable) {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg |= CH3_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg &= ~CH3_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      }

      break;
    case ANALOG_TOTAL:
      if (Enable) {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg |= DP_PHY_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        Reg  = AnalogixDpRegRead (Dp, PhyPdAddr);
        Reg &= ~DP_PHY_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      }

      break;
    case POWER_ALL:
      if (Enable) {
        Reg = DP_PHY_PD | AUX_PD | CH3_PD | CH2_PD |
              CH1_PD | CH0_PD;
        AnalogixDpRegWrite (Dp, PhyPdAddr, Reg);
      } else {
        AnalogixDpRegWrite (Dp, PhyPdAddr, 0x00);
      }

      break;
    default:
      break;
  }
}

VOID
AnalogixDpSetPllPowerDown (
  OUT struct AnalogixDpDevice  *Dp,
  BOOLEAN                      Enable
  )
{
  UINT32  Reg;

  if (Enable) {
    Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_PLL_CTL);
    Reg |= DP_PLL_PD;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_CTL, Reg);
  } else {
    Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_PLL_CTL);
    Reg &= ~DP_PLL_PD;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_PLL_CTL, Reg);
  }
}

VOID
AnalogixDpInitAnalogFunc (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  AnalogixDpSetAnalogPowerDown (Dp, POWER_ALL, 0);

  Reg = PLL_LOCK_CHG;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_STA_1, Reg);

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_DEBUG_CTL);
  Reg &= ~(F_PLL_LOCK | PLL_LOCK_CTRL);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_DEBUG_CTL, Reg);

  /* Power up PLL */
  AnalogixDpSetPllPowerDown (Dp, 0);

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_FUNC_EN_2);
  Reg &=  ~(SERDES_FIFO_FUNC_EN_N | LS_CLK_DOMAIN_FUNC_EN_N
            | AUX_FUNC_EN_N);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_FUNC_EN_2, Reg);
}

VOID
AnalogixDpClearHotplugInterrupts (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  // TODO
  Reg = HOTPLUG_CHG | HPD_LOST | PLUG;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_COMMON_INT_STA_4, Reg);

  Reg = INT_HPD;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA, Reg);
}

VOID
AnalogixDpInitHpd (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  AnalogixDpClearHotplugInterrupts (Dp);

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_3);
  Reg &= ~(F_HPD | HPD_CTRL);

  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_3, Reg);
}

VOID
AnalogixDpResetAux (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  /* Disable AUX channel module */
  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_FUNC_EN_2);
  Reg |= AUX_FUNC_EN_N;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_FUNC_EN_2, Reg);
}

VOID
AnalogixDpInitAux (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  /* Clear inerrupts related to AUX channel */
  Reg = RPLY_RECEIV | AUX_ERR;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA, Reg);

  AnalogixDpResetAux (Dp);

  Reg = AUX_BIT_PERIOD_EXPECTED_DELAY (0) |
        AUX_HW_RETRY_COUNT_SEL (3) |
        AUX_HW_RETRY_INTERVAL_600_MICROSECONDS;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_HW_RETRY_CTL, Reg);

  /* Receive AUX Channel DEFER commands equal to DEFFER_COUNT*64 */
  Reg = DEFER_CTRL_EN | DEFER_COUNT (1);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_DEFER_CTL, Reg);

  /* Enable AUX channel module */
  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_FUNC_EN_2);
  Reg &= ~AUX_FUNC_EN_N;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_FUNC_EN_2, Reg);
}

INT32
AnalogixDpWriteByteToDpcd (
  OUT struct AnalogixDpDevice  *Dp,
  IN UINT32                    RegAddr,
  IN UINT8                     Data
  )
{
  UINT32  Reg;
  UINTN   i;
  UINTN   Retval;

  for (i = 0; i < 3; i++) {
    /* Clear AUX CH data buffer */
    Reg = BUF_CLR;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUFFER_DATA_CTL, Reg);

    /* Select DPCD device address */
    Reg = AUX_ADDR_7_0 (RegAddr);
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_7_0, Reg);
    Reg = AUX_ADDR_15_8 (RegAddr);
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_15_8, Reg);
    Reg = AUX_ADDR_19_16 (RegAddr);
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_19_16, Reg);

    /* write Data buffer */
    Reg = (UINT32)Data;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUF_DATA_0, Reg);

    /*
     *  Set DisplayPort transaction and write 1 byte
     *  If bit 3 is 1, DisplayPort transaction.
     *  If Bit 3 is 0, I2C transaction.
     */
    Reg = AUX_TX_COMM_DP_TRANSACTION | AUX_TX_COMM_WRITE;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_1, Reg);

    Retval = AnalogixDpStartAuxTransaction (Dp);
    if (Retval == 0) {
      break;
    }
  }

  return Retval;
}

INT32
AnalogixDpWriteBytesToDpcd (
  OUT struct AnalogixDpDevice  *Dp,
  IN  UINT32                   RegAddr,
  IN  UINT32                   Count,
  IN  UINT8                    Data[]
  )
{
  UINT32  Reg;
  UINT32  StartOffset;
  UINT32  CurDataCount, CurDataIdx;
  INT32   Retval = 0;
  INT32   i;

  /* Clear AUX CH data buffer */
  Reg = BUF_CLR;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUFFER_DATA_CTL, Reg);

  StartOffset = 0;
  while (StartOffset < Count) {
    /* Buffer size of AUX CH is 16 * 4bytes */
    if ((Count - StartOffset) > 16) {
      CurDataCount = 16;
    } else {
      CurDataCount = Count - StartOffset;
    }

    for (i = 0; i < 3; i++) {
      /* Select DPCD device address */
      Reg = AUX_ADDR_7_0 (RegAddr + StartOffset);
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_7_0, Reg);
      Reg = AUX_ADDR_15_8 (RegAddr + StartOffset);
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_15_8, Reg);
      Reg = AUX_ADDR_19_16 (RegAddr + StartOffset);
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_19_16, Reg);

      for (CurDataIdx = 0; CurDataIdx < CurDataCount;
           CurDataIdx++)
      {
        Reg = Data[StartOffset + CurDataIdx];
        AnalogixDpRegWrite (
          Dp,
          ANALOGIX_DP_BUF_DATA_0 +
          4 * CurDataIdx,
          Reg
          );
      }

      /*
       * Set DisplayPort transaction and write
       * If bit 3 is 1, DisplayPort transaction.
       * If Bit 3 is 0, I2C transaction.
       */
      Reg = AUX_LENGTH (CurDataCount) |
            AUX_TX_COMM_DP_TRANSACTION | AUX_TX_COMM_WRITE;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_1, Reg);

      /* Start AUX transaction */
      Retval = AnalogixDpStartAuxTransaction (Dp);
      if (Retval == 0) {
        break;
      }
    }

    StartOffset += CurDataCount;
  }

  return Retval;
}

INT32
AnalogixDpReadByteFromDpcd (
  OUT  struct AnalogixDpDevice  *Dp,
  IN   UINT32                   RegAddr,
  IN   UINT8                    *Data
  )
{
  UINT32  Reg;
  INT32   i;
  INT32   Retval;

  for (i = 0; i < 3; i++) {
    /* Clear AUX CH data buffer */
    Reg = BUF_CLR;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUFFER_DATA_CTL, Reg);

    /* Select DPCD device address */
    Reg = AUX_ADDR_7_0 (RegAddr);
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_7_0, Reg);
    Reg = AUX_ADDR_15_8 (RegAddr);
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_15_8, Reg);
    Reg = AUX_ADDR_19_16 (RegAddr);
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_19_16, Reg);

    /*
     * Set DisplayPort transaction and read 1 byte
     * If bit 3 is 1, DisplayPort transaction.
     * If Bit 3 is 0, I2C transaction.
     */
    Reg = AUX_TX_COMM_DP_TRANSACTION | AUX_TX_COMM_READ;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_1, Reg);

    /* Start AUX transaction */
    Retval = AnalogixDpStartAuxTransaction (Dp);
    if (Retval == 0) {
      break;
    }
  }

  /* Read data buffer */
  Reg   = AnalogixDpRegRead (Dp, ANALOGIX_DP_BUF_DATA_0);
  *Data = (UINT8)(Reg & 0xff);

  return Retval;
}

INT32
AnalogixDpStartAuxTransaction (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  INT32  Reg;
  INT32  Retval      = 0;
  INT32  TimeOutLoop = 0;

  /* Enable AUX CH operation */
  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_AUX_CH_CTL_2);
  Reg |= AUX_EN;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_2, Reg);

  /* Is AUX CH command reply received? */
  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_INT_STA);

  while (!(Reg & RPLY_RECEIV)) {
    TimeOutLoop++;
    if (DP_TIMEOUT_LOOP_COUNT < TimeOutLoop) {
      DEBUG ((DEBUG_WARN, "AUX CH command reply failed!\n"));
      return -ETIMEDOUT;
    }

    Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_INT_STA);
    NanoSecondDelay (11000);
  }

  /* Clear interrupt source for AUX CH command reply */
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA, Reg);

  /* Clear interrupt source for AUX CH access error */
  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_INT_STA);
  if (Reg & AUX_ERR) {
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_INT_STA, AUX_ERR);
    return -EREMOTEIO;
  }

  /* Check AUX CH error access status */
  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_AUX_CH_STA);
  if ((Reg & AUX_STATUS_MASK) != 0) {
    DEBUG ((
      DEBUG_ERROR,
      "AUX CH error happens: %d \n",
      Reg & AUX_STATUS_MASK
      ));
    return -EREMOTEIO;
  }

  return Retval;
}

INT32
AnalogixDpReadBytesFromDpcd (
  OUT struct AnalogixDpDevice  *Dp,
  IN  UINT32                   RegAddr,
  IN  UINT32                   Count,
  IN  UINT8                    Data[]
  )
{
  UINT32  Reg;
  UINT32  StartOffset;
  UINT32  CurDataCount, CurDataIdx;
  INT32   i;
  INT32   Retval = 0;

  /* Clear AUX CH data buffer*/
  Reg = BUF_CLR;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUFFER_DATA_CTL, Reg);

  StartOffset = 0;
  while (StartOffset < Count) {
    /* Buffer size of AUX CH is 16 * 4 bytes */
    if ((Count - StartOffset) > 16) {
      CurDataCount = 16;
    } else {
      CurDataCount = Count - StartOffset;
    }

    /* AUX CH Request Transaction process */
    for (i = 0; i < 3; i++) {
      /* Select DPCD device address */
      Reg = AUX_ADDR_7_0 (RegAddr + StartOffset);
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_7_0, Reg);
      Reg = AUX_ADDR_15_8 (RegAddr + StartOffset);
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_15_8, Reg);
      Reg = AUX_ADDR_19_16 (RegAddr + StartOffset);
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_19_16, Reg);

      /*
       * Set DisplayPort transaction and read
       * If bit 3 is 1,DisplayPort transaction
       * If bit 3 is 0,I2C transaction
       */
      Reg = AUX_LENGTH (CurDataCount) |
            AUX_TX_COMM_DP_TRANSACTION | AUX_TX_COMM_READ;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_1, Reg);

      /* Start AUX transaction */
      Retval = AnalogixDpStartAuxTransaction (Dp);
      if (Retval == 0) {
        break;
      }
    }

    for (CurDataIdx = 0; CurDataIdx < CurDataCount;
         CurDataIdx++)
    {
      Reg = AnalogixDpRegRead (
              Dp,
              ANALOGIX_DP_BUF_DATA_0
              + 4 * CurDataIdx
              );
      Data[StartOffset + CurDataIdx] =
        (UINT8)Reg;
    }

    StartOffset += CurDataCount;
  }

  return Retval;
}

INTN
AnalogixDpSelectI2cDevice (
  OUT struct AnalogixDpDevice  *Dp,
  UINTN                        DeviceAddr,
  UINTN                        RegAddr
  )
{
  UINT32  Reg;
  INTN    Retval;

  // Todo
  /* Set EDID device address */
  Reg = DeviceAddr;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_7_0, Reg);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_15_8, 0x0);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_ADDR_19_16, 0x0);

  /* Set offset from base address of EDID device */
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUF_DATA_0, RegAddr);

  /*
   * Set I2C transaction and write address
   * If bit 3 is 1, DisplayPort transaction.
   * If Bit 3 is 0, I2C transaction.
   */
  Reg = AUX_TX_COMM_I2C_TRANSACTION | AUX_TX_COMM_MOT |
        AUX_TX_COMM_WRITE;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_1, Reg);

  /* Start AUX transaction */
  Retval = AnalogixDpStartAuxTransaction (Dp);
  if (Retval < 0) {
    return Retval;
  }

  return 0;
}

INTN
AnalogixDpReadByteFromI2c (
  struct AnalogixDpDevice  *Dp,
  UINTN                    DeviceAddr,
  UINTN                    RegAddr,
  UINTN                    *DATA
  )
{
  UINT32  Reg;
  INTN    i;
  INTN    Retval;

  // Todo
  for (i = 0; i < 3; i++) {
    /* Clear AUX CH data buffer */
    Reg = BUF_CLR;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUFFER_DATA_CTL, Reg);

    /* Select EDID device */
    Retval = AnalogixDpSelectI2cDevice (
               Dp,
               DeviceAddr,
               RegAddr
               );
    if (Retval != 0) {
      continue;
    }

    /*
     * Set I2C transaction and read data
     * If bit 3 is 1, DisplayPort transaction.
     * If Bit 3 is 0, I2C transaction.
     */
    Reg = AUX_TX_COMM_I2C_TRANSACTION |
          AUX_TX_COMM_READ;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_1, Reg);

    /* Start AUX transaction */
    Retval = AnalogixDpStartAuxTransaction (Dp);
    if (Retval == 0) {
      break;
    }
  }

  /* Read data */
  if (Retval == 0) {
    *DATA = AnalogixDpRegRead (Dp, ANALOGIX_DP_BUF_DATA_0);
  }

  return Retval;
}

INTN
AnalogixDpReadBytesFromI2c (
  OUT struct AnalogixDpDevice  *Dp,
  UINTN                        DeviceAddr,
  UINTN                        RegAddr,
  UINTN                        Count,
  UINT8                        Edid[]
  )
{
  UINT32  Reg;
  UINTN   i, j;
  UINTN   CurDataIdx;
  UINTN   Defer  = 0;
  INT32   Retval = 0;

  // Todo
  for (i = 0; i < Count; i += 16) {
    for (j = 0; j < 3; j++) {
      /* Clear AUX CH data buffer */
      Reg = BUF_CLR;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_BUFFER_DATA_CTL, Reg);

      /* Set normal AUX CH command */
      Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_AUX_CH_CTL_2);
      Reg &= ~ADDR_ONLY;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_AUX_CH_CTL_2, Reg);

      /*
       * If Rx sends defer, Tx sends only reads
       * request without sending address
       */
      if (!Defer) {
        Retval = AnalogixDpSelectI2cDevice (
                   Dp,
                   DeviceAddr,
                   RegAddr + i
                   );
      } else {
        Defer = 0;
      }

      if (Retval == 0) {
        /*
         * Set I2C transaction and write data
         * If bit 3 is 1, DisplayPort transaction.
         * If Bit 3 is 0, I2C transaction.
         */
        Reg = AUX_LENGTH (16) |
              AUX_TX_COMM_I2C_TRANSACTION |
              AUX_TX_COMM_READ;
        AnalogixDpRegWrite (
          Dp,
          ANALOGIX_DP_AUX_CH_CTL_1,
          Reg
          );

        /* Start AUX transaction */
        Retval = AnalogixDpStartAuxTransaction (Dp);
        if (Retval == 0) {
          break;
        }
      }

      /* Check if Rx sends defer */
      Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_AUX_RX_COMM);
      if ((Reg == AUX_RX_COMM_AUX_DEFER) ||
          (Reg == AUX_RX_COMM_I2C_DEFER))
      {
        Defer = 1;
      }
    }

    for (CurDataIdx = 0; CurDataIdx < 16; CurDataIdx++) {
      Reg = AnalogixDpRegRead (
              Dp,
              ANALOGIX_DP_BUF_DATA_0
              + 4 * CurDataIdx
              );
      Edid[i + CurDataIdx] = (UINT8)Reg;
    }
  }

  return Retval;
}

VOID
AnalogixDpSetTrainingPattern (
  OUT struct AnalogixDpDevice  *Dp,
  OUT enum PatternSet          Pattern
  )
{
  UINT32  Reg;

  switch (Pattern) {
    case PRBS7:
      Reg = SCRAMBLING_ENABLE | LINK_QUAL_PATTERN_SET_PRBS7;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
      break;
    case D10_2:
      Reg = SCRAMBLING_ENABLE | LINK_QUAL_PATTERN_SET_D10_2;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
      break;
    case TRAINING_PTN1:
      Reg = SCRAMBLING_DISABLE | SW_TRAINING_PATTERN_SET_PTN1;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
      break;
    case TRAINING_PTN2:
      Reg = SCRAMBLING_DISABLE | SW_TRAINING_PATTERN_SET_PTN2;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
      break;
    case TRAINING_PTN3:
      Reg = SCRAMBLING_DISABLE | SW_TRAINING_PATTERN_SET_PTN3;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
      break;
    case DP_NONE:
      Reg = SCRAMBLING_ENABLE |
            LINK_QUAL_PATTERN_SET_DISABLE |
            SW_TRAINING_PATTERN_SET_NORMAL;
      AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
    default:
      break;
  }
}

UINT32
AnalogixDpGetLaneLinkTraining (
  OUT struct AnalogixDpDevice  *Dp,
  IN UINT8                     Lane
  )
{
  return AnalogixDpRegRead (
           Dp,
           ANALOGIX_DP_LN0_LINK_TRAINING_CTL + 4 * Lane
           );
}

VOID
AnalogixDpGetLinkBandwidth (
  OUT struct AnalogixDpDevice  *Dp,
  UINT32                       *BwType
  )
{
  UINT32  Reg;

  Reg     = AnalogixDpRegRead (Dp, ANALOGIX_DP_LINK_BW_SET);
  *BwType = Reg;
}

VOID
AnalogixDpGetLaneCount (
  OUT struct AnalogixDpDevice  *Dp,
  UINT32                       *Count
  )
{
  UINT32  Reg;

  Reg    = AnalogixDpRegRead (Dp, ANALOGIX_DP_LANE_COUNT_SET);
  *Count = Reg;
}

INT32
AnalogixDpIsSlaveVideoStreamClockOn (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_1);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_1, Reg);

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_1);

  if (!(Reg & DET_STA)) {
    return -EINVAL;
  }

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_2);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_2, Reg);

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_2);

  if (Reg & CHA_STA) {
    return -EINVAL;
  }

  return 0;
}

VOID
AnalogixDpEnableVideoMaster (
  OUT struct AnalogixDpDevice  *Dp,
  BOOLEAN                      Enable
  )
{
  UINT32  Reg;

  if (Enable) {
    Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_SOC_GENERAL_CTL);
    Reg &= ~VIDEO_MODE_MASK;
    Reg |= VIDEO_MASTER_MODE_EN | VIDEO_MODE_MASTER_MODE;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_SOC_GENERAL_CTL, Reg);
  } else {
    Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_SOC_GENERAL_CTL);
    Reg &= ~VIDEO_MODE_MASK;
    Reg |= VIDEO_MODE_SLAVE_MODE;
    AnalogixDpRegWrite (Dp, ANALOGIX_DP_SOC_GENERAL_CTL, Reg);
  }
}

VOID
AnalogixDpStartVideo (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_VIDEO_CTL_1);
  Reg |= VIDEO_EN;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_VIDEO_CTL_1, Reg);
}

INTN
AnalogixDpIsVideoStreamOn (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_3);
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_SYS_CTL_3, Reg);

  Reg = AnalogixDpRegRead (Dp, ANALOGIX_DP_SYS_CTL_3);
  if (!(Reg & STRM_VALID)) {
    return -EINVAL;
  }

  return 0;
}

VOID
AnalogixDpEnableScrambling (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_TRAINING_PTN_SET);
  Reg &= ~SCRAMBLING_DISABLE;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
}

VOID
AnalogixDpDisableScrambling (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  Reg;

  Reg  = AnalogixDpRegRead (Dp, ANALOGIX_DP_TRAINING_PTN_SET);
  Reg |= ~SCRAMBLING_DISABLE;
  AnalogixDpRegWrite (Dp, ANALOGIX_DP_TRAINING_PTN_SET, Reg);
}

VOID
AnalogixDpSetVideoFormat (
  OUT struct AnalogixDpDevice  *Dp,
  OUT CONST DRM_DISPLAY_MODE   *Mode
  )
{
  UINT32  Hsw, Hfp, Hbp, Vsw, Vfp, Vbp;

  Dp->VideoInfo.Interlaced    = !!(Mode->Flags & DRM_MODE_FLAG_INTERLACE);
  Dp->VideoInfo.VSyncPolarity = TRUE;
  Dp->VideoInfo.HSyncPolarity = TRUE;

  Hsw = Mode->HSyncEnd - Mode->HSyncStart;
  Hfp = Mode->HSyncStart - Mode->HDisplay;
  Hbp = Mode->HTotal - Mode->HSyncEnd;
  Vsw = Mode->VSyncEnd - Mode->VSyncStart;
  Vfp = Mode->VSyncStart - Mode->VDisplay;
  Vbp = Mode->VTotal - Mode->VSyncEnd;

  /* Set Video Format Parameters */
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_TOTAL_LINE_CFG_L,
    TOTAL_LINE_CFG_L (Mode->VTotal)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_TOTAL_LINE_CFG_H,
    TOTAL_LINE_CFG_H (Mode->VTotal >> 8)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_ACTIVE_LINE_CFG_L,
    ACTIVE_LINE_CFG_L (Mode->VDisplay)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_ACTIVE_LINE_CFG_H,
    ACTIVE_LINE_CFG_H (Mode->VDisplay >> 8)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_V_F_PORCH_CFG,
    V_F_PORCH_CFG (Vfp)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_V_SYNC_WIDTH_CFG,
    V_SYNC_WIDTH_CFG (Vsw)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_V_B_PORCH_CFG,
    V_B_PORCH_CFG (Vbp)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_TOTAL_PIXEL_CFG_L,
    TOTAL_PIXEL_CFG_L (Mode->HTotal)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_TOTAL_PIXEL_CFG_H,
    TOTAL_PIXEL_CFG_H (Mode->HTotal >> 8)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_ACTIVE_PIXEL_CFG_L,
    ACTIVE_PIXEL_CFG_L (Mode->HDisplay)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_ACTIVE_PIXEL_CFG_H,
    ACTIVE_PIXEL_CFG_H (Mode->HDisplay >> 8)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_H_F_PORCH_CFG_L,
    H_F_PORCH_CFG_L (Hfp)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_H_F_PORCH_CFG_H,
    H_F_PORCH_CFG_H (Hfp >> 8)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_H_SYNC_CFG_L,
    H_SYNC_CFG_L (Hsw)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_H_SYNC_CFG_H,
    H_SYNC_CFG_H (Hsw >> 8)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_H_B_PORCH_CFG_L,
    H_B_PORCH_CFG_L (Hbp)
    );
  AnalogixDpRegWrite (
    Dp,
    ANALOGIX_DP_H_B_PORCH_CFG_H,
    H_B_PORCH_CFG_H (Hbp >> 8)
    );
}

VOID
DumpHdptxPhyRegisters (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINT32  i, Ret;
  UINT32  Shift, BASE;

  if (!Dp->Id) {
    Shift = BASE = EDP0TX_PHY_BASE;
  } else {
    Shift = BASE = EDP1TX_PHY_BASE;
  }

  DEBUG ((DEBUG_WARN, "\nHDPTX_PHY_BASE\n%.8x ", BASE));
  for (i = 0; i < 168; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }

  Shift = BASE+0x400;
  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for (i = 0; i < 40; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }

  Shift = BASE + 0x800;
  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for (i = 0; i < 40; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }

  Shift = BASE + 0x0C00;
  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for (i = 0; i < 40; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }

  Shift = BASE + 0x1000;
  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for ( i = 0; i < 40; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }

  Shift = BASE + 0x1400;
  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for (i = 0; i < 40; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }

  Shift = BASE + 0x1800;
  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for (i = 0; i < 40; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }
}

VOID
DumpDpRegisters (
  OUT struct AnalogixDpDevice  *Dp
  )
{
  UINTN   i;
  UINT32  Ret;
  UINT32  Shift;

  Shift = Dp->Base;

  DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
  for (i = 0; i < 800; i++) {
    Ret   = MmioRead32 (Shift);
    Shift = Shift + 0x04;
    DEBUG ((DEBUG_WARN, "%.8x ", Ret));
    if ((i+1) % 4 == 0) {
      DEBUG ((DEBUG_WARN, "\n%.8x ", Shift));
    }
  }
}
